#include <cmath>
#include <iostream>
#include "LoadCalculator.h"

#include "Utility/Logging/SGLogger.h"
#include "Utility/Algebra/Point.h"
#include "Utility/Algebra/Vector3D.h"
#include "Utility/Algebra/Tools.h"
#include "DataStructure/Common/Real.h"
#include "DataStructure/Common/Id.h"
#include "DataStructure/Common/Constant.h"
#include "DataStructure/Input/Element/ElementTypes.h"
#include "DataStructure/Input/Load/PLOADData.h"

#include "DBManager/IService/Input/Element/IElementService.h"
#include "DBManager/IService/Input/Node/INodeService.h"
#include "DBManager/IService/Input/Load/ILoadCombinationService.h"
#include "DBManager/IService/Input/Load/ILoadSummaryService.h"
#include "DBManager/IService/Input/Load/IFORCEService.h"
#include "DBManager/IService/Input/Load/IFORCE1Service.h"
#include "DBManager/IService/Input/Load/IFORCE2Service.h"
#include "DBManager/IService/Input/Load/IMOMENTService.h"
#include "DBManager/IService/Input/Load/IMOMENT1Service.h"
#include "DBManager/IService/Input/Load/IMOMENT2Service.h"
#include "DBManager/IService/Input/Load/ISLOADService.h"
#include "DBManager/IService/Input/Load/IQBDY3Service.h"
#include "DBManager/IService/Input/Load/IPLOADService.h"
#include "DBManager/IService/Input/Load/IPLOAD1Service.h"
#include "DBManager/IService/Input/Load/IPLOAD2Service.h"
#include "DBManager/IService/Input/Load/IPLOAD4Service.h"

// #include "DBManager/IService/Input/Element/ICHBDYPService.h"
// #include "DBManager/IService/Input/Element/ICHBDYGService.h"

#include "SGFem/Integration/IntegrationFormula/Gauss2DTria3P.h"
#include "SGFem/Integration/IntegrationFormula/Gauss2D2X2P.h"
#include "SGFem/Integration/ShapeFunction/Plane4NodeShpFunc.h"
#include "SGFem/Pre/Assembly/Assembly.h"
#include "SGFem/Pre/Assembly/ModelInfoQuery.h"
#include "SGFem/Pre/Assembly/CoordinateProcessor.h"
#include "SGFem/BeamSecPropCalculator/SectionParser/BarSectionParser.h"

#include "SGFem/ElementCalculator/ThermalElement/CHBDYPLINECalculator.h"
#include "SGFem/ElementCalculator/ThermalElement/CHBDYGAREACalculator.h"
#include "SGFem/ElementCalculator/Common/ElementTools.hpp"
#include "SGFem/ElementCalculator/BarElement/BarCalculator.h"

using SG::Algebra::Matrix;
using SG::Algebra::Matrixd;

using SG::Algebra::Point;
using SG::Algebra::PointArray;
using SG::Algebra::Vector3D;
using SG::DataStructure::Common::Id;
using SG::DataStructure::Common::IdArray;
using SG::DataStructure::Common::Real;
using SG::DataStructure::Common::Status;
using SG::DataStructure::Common::VIRTUAL_GRID_ID_ORIGIN;
using SG::DataStructure::Common::TOLERANCE;
using SG::DataStructure::FEM::ElementTypes;
using SG::DataStructure::FEM::FORCEData;
using SG::DataStructure::FEM::NodeData;
using SG::DataStructure::FEM::CBARData;
using SG::DataStructure::FEM::CBEAMData;
using SG::DataStructure::FEM::PLOADData;
using SG::DataStructure::FEM::IntegPointArray;
using namespace SG::DataStructure::FEM;
using namespace SG::DBManager;
using SG::FEMSolver::EigenVector;
using SG::FEMSolver::EigenSparse;
using SG::FEMSolver::AssemblyEigen;
using SG::FEMSolver::CoordinateProcessor;
using SG::FEMSolver::ModelInfoQuery;


using namespace SG::FEMSolver::Element;


void LoadCalculator::ForceCalc (const CoordinateProcessor& coordHandler,
                                              const FORCEData&           force,
                                              const NodeData&            node,
                                              Matrix& _OUT               loadMatrix)
{
    auto load = force.GetLoad ();

    Matrix TG (3, 3);
    Point point;
    Id    dispCoord;

    // tanslate to basic coordinate
    if (0 != force.m_coordSysmId)
    {
        // 载荷作用点在Basic坐标下的分量
        coordHandler.GetGlobalCoordinate (node, _OUT point, _OUT dispCoord);

        // 旋转载荷方向
        coordHandler.GetCoordTrans (force.m_coordSysmId, point, _OUT TG);
        TG   = TG.Transpose ();
        load = load.Rotate (TG);
    }

    // translate load to grid analysis coordinate
    dispCoord = node.m_dispCoord;
    if (0 != dispCoord)
    {
        // 载荷作用点在Basic坐标下的分量
        coordHandler.GetGlobalCoordinate (node, _OUT point, _OUT dispCoord);
        
        coordHandler.GetCoordTrans (dispCoord, point, _OUT TG);
        load = load.Rotate (TG);
    }


    const auto fi = force.m_factor;
    loadMatrix    = Matrix (3, 1, { load.m_x * fi, load.m_y * fi, load.m_z * fi });
}

void LoadCalculator::PLoadCalc (const std::shared_ptr<SG::DBManager::INodeService>& InodeService,
                                              const SG::DataStructure::FEM::PLOADData&            pLoad,
                                              SG::Algebra::Matrix& _OUT                           loadMatrix)
{
    const auto& coordProcessor = CoordinateProcessor::GetInstance ();
    
    // Node coordinate
    const std::size_t        nPoint = (pLoad.m_G[3] == 0) ? 3 : 4;
    PointArray               points (nPoint);
    std::vector<std::size_t> nodeIndexes (nPoint);
    IdArray                  dispCoordIds (nPoint);
    ModelInfoQuery::QueryNode (InodeService, pLoad.m_G,  nPoint, points, nodeIndexes, dispCoordIds);

    /**  @brief    计算三角形面片上压力载荷，此接口调用者需要自行考虑是否需要对 loadVector进行初始化
     *
     */
    auto PloadTriCalc = [&coordProcessor, &points, &dispCoordIds] (Real P, const size_t* const posi, Matrix& loadVector) -> void 
    {
        // 计算单元面积
        Vector3D r12 = Distance (points[posi[1]], points[posi[0]]);
        Vector3D r13 = Distance (points[posi[2]], points[posi[0]]);
        Vector3D n   = Cross (r12, r13);
        Real     area;
        n.Normalize (area);
        area = area / 2.0;

        n = n * (P * area / 3.0);

        // Matrix load;
        for (std::size_t i = 0; i < 3; ++i)
        {
            const auto nodeIndex = posi[i];
            auto posi0 = nodeIndex * 3;
            if (0 != dispCoordIds[posi[i]])
            {
                //   载荷从全局坐标转换成单元局部坐标系
                Matrix TG (3, 3);
                coordProcessor.GetCoordTrans (dispCoordIds[nodeIndex], points[nodeIndex], _OUT TG);
                n = n.Rotate (TG);
            }
            loadVector (posi0, 0) += n.m_x;
            loadVector (posi0 + 1, 0) += n.m_y;
            loadVector (posi0 + 2, 0) += n.m_z;
        }
    };

    // 判断是三角形单元还是四边形单元
    if (pLoad.m_G[3] == 0)
    {
        // 计算等效力，取等效力的1/3平均分配在三角形的每个节点上
        loadMatrix          = Matrix (9, 1);
        std::size_t posi[3] = { 0, 1, 2 };
        PloadTriCalc (pLoad.m_P, posi, loadMatrix);
    }
    else
    {
        loadMatrix = Matrix (12, 1);
        // 按照四个三角形叠加
        std::size_t tri1[3] = { 0, 1, 2 };
        PloadTriCalc (pLoad.m_P, tri1, loadMatrix);
        std::size_t tri2[3] = { 0, 2, 3 };
        PloadTriCalc (pLoad.m_P, tri2, loadMatrix);
        std::size_t tri3[3] = { 0, 1, 3 };
        PloadTriCalc (pLoad.m_P, tri3, loadMatrix);
        std::size_t tri4[3] = { 1, 2, 3 };
        PloadTriCalc (pLoad.m_P, tri4, loadMatrix);

        // 每个节点的贡献计算了两次
        loadMatrix = loadMatrix * 0.5;
    }
}

void CalculateLoadMatrix( const std::array<Real, 5>& I1, const std::array<Real, 5>& I2,        
    const Real* const constVals, const Real* const slopeVals, 
                                          Real length, Real S2, Real S3, Matrix& _OUT loadMatrix)
{
    const Real DT = length * length;

    loadMatrix(0, 0) = I1[0] * constVals[0] + I2[0] * slopeVals[0];                   
    loadMatrix(1, 0) = I1[1] * constVals[1] + I2[1] * slopeVals[1];                   
    loadMatrix(2, 0) = I1[1] * constVals[2] + I2[1] * slopeVals[2];                   
    loadMatrix(3, 0) = 0.0;                                                           
    loadMatrix(4, 0) = -length * (I1[3] * constVals[2] + I2[3] * slopeVals[2]);        
    loadMatrix(5, 0) = length * (I1[3] * constVals[1] + I2[3] * slopeVals[1]);        
    loadMatrix(6, 0) = length * S2 * constVals[0] + DT * S3 * slopeVals[0];           
    loadMatrix(7, 0) = I1[2] * constVals[1] + I2[2] * slopeVals[1];                   
    loadMatrix(8, 0) = I1[2] * constVals[2] + I2[2] * slopeVals[2];                   
    loadMatrix(9, 0) = 0.0;                                                           
    loadMatrix(10, 0) = -length * (I1[4] * constVals[2] + I2[4] * slopeVals[2]);       
    loadMatrix(11, 0) = length * (I1[4] * constVals[1] + I2[4] * slopeVals[1]);       
}

// 这个匿名空间封装了一些用于计算pload1的函数
namespace
{
    // 计算集中FORCE荷载的匿名函数
    auto calcConcentratedForce = [](Real x1, Real length, const Matrix& load, Real OMALY, Real OMALZ, Matrix& _OUT loadMatrix) -> void 
    {
        const Real temp = 1.0 - x1;
        const Real ALY = 1.0 - OMALY;  
        const Real ALZ = 1.0 - OMALZ;  

        Real temp1 = 1.0 - 3.0 * x1 * x1 + 2.0 * x1 * x1 * x1;
        loadMatrix(0, 0) = temp * load(0, 0);
        loadMatrix(1, 0) = temp1 * load(1, 0) * OMALY + load(1, 0) * temp * ALY;
        loadMatrix(2, 0) = temp1 * load(2, 0) * OMALZ + load(2, 0) * temp * ALZ;
        loadMatrix(3, 0) = 0.0;

        temp1 = -length * x1 * temp * temp;
        const Real temp2 = temp * length * x1 * 0.5;  // 复用temp计算临时变量
        loadMatrix(4, 0) = temp1 * load(2, 0) * OMALZ - load(2, 0) * temp2 * ALZ;
        loadMatrix(5, 0) = -temp1 * load(1, 0) * OMALY + load(1, 0) * temp2 * ALY;

        temp1 = 3.0 * x1 * x1 - 2.0 * x1 * x1 * x1;
        loadMatrix(6, 0) = x1 * load(0, 0);
        loadMatrix(7, 0) = temp1 * load(1, 0) * OMALY + load(1, 0) * x1 * ALY;
        loadMatrix(8, 0) = temp1 * load(2, 0) * OMALZ + load(2, 0) * x1 * ALZ;
        loadMatrix(9, 0) = 0.0;

        temp1 = (1.0 - x1) * length * x1 * x1;
        loadMatrix(10, 0) = temp1 * load(2, 0) * OMALZ + load(2, 0) * temp2 * ALZ;
        loadMatrix(11, 0) = -temp1 * load(1, 0) * OMALY - load(1, 0) * temp2 * ALY;
    };

    // 计算集中MOMENT荷载的匿名函数
    auto calcConcentratedMoment = [](Real x1, Real length, const Matrix& load, Real OMALY, Real OMALZ, Matrix& _OUT loadMatrix) -> void 
    {
        const Real temp = 1.0 - x1;
        const Real ALY = 1.0 - OMALY;  
        const Real ALZ = 1.0 - OMALZ;  

        Real temp1 = -6.0 / length * x1 * temp;
        loadMatrix(0, 0) = 0.0;
        loadMatrix(1, 0) = temp1 * load(2, 0) * OMALY;
        loadMatrix(2, 0) = -temp1 * load(1, 0) * OMALZ;
        loadMatrix(3, 0) = temp * load(0, 0);

        temp1 = 1.0 - 4.0 * x1 + 3.0 * x1 * x1;
        loadMatrix(4, 0) = temp1 * load(1, 0) * OMALZ + load(1, 0) * temp * ALZ;
        loadMatrix(5, 0) = temp1 * load(2, 0) * OMALY + load(2, 0) * temp * ALY;

        loadMatrix(6, 0) = 0.0;
        loadMatrix(7, 0) = -loadMatrix(1, 0);
        loadMatrix(8, 0) = -loadMatrix(2, 0);
        loadMatrix(9, 0) = x1 * load(0, 0);

        temp1 = 3.0 * x1 * x1 - 2.0 * x1;
        loadMatrix(10, 0) = temp1 * load(1, 0) * OMALZ + load(1, 0) * x1 * ALZ;
        loadMatrix(11, 0) = temp1 * load(2, 0) * OMALY + load(2, 0) * x1 * ALY;
    };
}

void LoadCalculator::PLoad1Calc (const SG::DBManager::DBServiceFactorySP             dbServiceFactory,
                                 const SG::DataStructure::FEM::PLOAD1Data&           pLoad1,
                                 SG::Algebra::Matrix& _OUT                           loadMatrix,
                                 std::vector<std::size_t>& _OUT                      nodeIndexes)
{
    const auto& eleService = dbServiceFactory->get<IElementService> ();
    SG::FEMSolver::Element::BarSectionParser secParser;
    PBARData property;
    IsotropicMaterial material;

    auto x1 = pLoad1.m_x1;
    auto x2 = pLoad1.m_x2;

    loadMatrix = Matrix (12, 1);

    //单元类型
    const auto  elelist = eleService->FindElement (pLoad1.m_eId);
    switch (elelist)
    {
        case ElementTypes::ELETYPE_Bar:
        {
            // 获取截面和材料属性
            CBARData    barData;
            eleService -> FindById (pLoad1.m_eId, barData);
            secParser.GetSectionProperty (dbServiceFactory, barData.m_pId, _OUT property);
            secParser.GetMatrial (property.m_mId, _OUT material);
            // 获取节点索引
            nodeIndexes.resize(2);
            auto& assembly = SG::FEMSolver::AssemblyEigen::GetInstance ();
            assembly.FindNodeIndex (barData.m_g, _OUT nodeIndexes);

            // 计算单元长度、方向转换矩阵（单元局部坐标系->basic坐标系）
            Real        length;                     //单元长度
            Matrix      T (3, 3);                   //方向转换矩阵（basic坐标系 ->单元局部坐标系）
            Matrix      TEle (12, 12);              //整体变换矩阵（global坐标系->单元局部坐标系）
            BarCalculator::BarCoorTrans(dbServiceFactory, barData,  _OUT TEle, _OUT T, _OUT  length);
            if (length == 0)
            {
                SGError << "The length of the bar element can't be zero, which id is " << pLoad1.m_eId;
            }

            // 计算横向剪切相关参数
            Real OMALY{1.0}, ALY{0.0}, OMALZ{1.0}, ALZ{0.0};
            if(fabs(property.m_K1) > TOLERANCE)
            {
                Real temp  = 12.0 * material.m_E * property.m_I1 / (property.m_K1 * material.m_G * property.m_A * length * length);
                OMALY = 1.0 / (temp + 1.0); 
                ALY   = 1.0 - OMALY;
            }

            if(fabs(property.m_K2) > TOLERANCE)
            {
                Real temp  = 12.0 * material.m_E * property.m_I2 / (property.m_K2 * material.m_G * property.m_A * length * length);
                OMALZ = 1.0 / (temp + 1.0);
                ALZ   = 1.0 - OMALZ;
            }

            // 建立单元坐标系下F1 F2的荷载向量, 前3个元素为F1 后三个向量为F2
            auto load = pLoad1.getLoad(T);

            // X1 X2归一化
            if ((1 == pLoad1.m_scale) or (3 == pLoad1.m_scale))
            {
                x1 = pLoad1.m_x1 / length; 
                x2 = pLoad1.m_x2 / length;
            }

            // 计算等效节点荷载
            if (!pLoad1.Isconcentration())
            {
                // pload1为分布荷载
                // 处理投影
                if ((pLoad1.m_scale > 2) and (pLoad1.isBasicCoord())) // 当scale为3 4且type为basic坐标系时需要处理投影
                {
                    const Real fScale = std::sqrt(1.0 - (T(0,pLoad1.m_type % 6) * T(0,pLoad1.m_type % 6)));
                    load =  load * fScale; 
                }

                // 积分基项，为后续积分做准备
                const Real S1 = x2 - x1;                                  // 0次
                const Real S2 = 0.5 * (pow(x2, 2) - pow(x1, 2));          // 1次
                const Real S3 = (1.0 / 3.0) * (pow(x2, 3) - pow(x1, 3));  // 2次
                const Real S4 = 0.25 * (pow(x2, 4) - pow(x1, 4));         // 3次
                
                Real I01 {0.0}, I02 {0.0}, I11 {0.0}, I21 {0.0}, I31 {0.0}, I41 {0.0}, I12 {0.0}, I22 {0.0}, I32 {0.0}, I42 {0.0}; // 积分参数
                Real avage[3] = { 0.0 }; // 常量项
                Real slope[3] = { 0.0 }; // 斜率项；
                Real DT {0.0};

                if (pLoad1.isForce())
                {
                    // 荷载类型为force
                    I01 = length * (S1 - S2);
                    I11 = length * (S1 - 3.0*S3 + 2.0*S4);
                    I21 = length * (3.0*S3 - 2.0*S4);
                    I31 = length * (S2 - 2.0*S3 + S4);
                    I41 = length * (S4 - S3);
                    DT  = length * length;
                    I02  = DT*(S2-S3);

                    if(fabs(pLoad1.m_p1 - pLoad1.m_p2) > TOLERANCE)    
                    {
                        // 线性分布force荷载
                        const Real S5 = 0.2 * (pow(x2, 5) - pow(x1, 5)); // 4次
                        I12 = DT * (S2 - 3.0*S4 + 2.0*S5);
                        I22 = DT * (3.0*S4 - 2.0*S5);
                        I32 = DT * (S3- 2.0*S4 + S5);
                        I42 = DT * (S5- S4);
                        DT = length * (x2 - x1);
                        slope[0] = (load(3,0) - load(0,0)) / DT;
                        slope[1] = (load(4,0) - load(1,0)) / DT;
                        slope[2] = (load(5,0) - load(2,0)) / DT;
                        avage[0] = load (0, 0) - x1*slope[0]*length;
                        avage[1] = load (1, 0) - x1*slope[1]*length;
                        avage[2] = load (2, 0) - x1*slope[2]*length;
                    }
                    else
                    {
                        //均布force荷载
                        avage[0] = load (0, 0);
                        avage[1] = load (1, 0);
                        avage[2] = load (2, 0);
                    }

                    // 计算等效节点荷载
                    std::array<Real, 5> I1 = {I01, I11, I21, I31, I41};
                    std::array<Real, 5> I2 = {I02, I12, I22, I32, I42};

                    CalculateLoadMatrix(I1, I2, avage, slope, length, S2, S3, loadMatrix);
                    
                    // 荷载类型为FORCE，剪切修正
                    DT = length * length;
                    if (ALY > TOLERANCE )    
                    {
                        const auto& avargeY = avage[1];
                        loadMatrix(1, 0) = OMALY * loadMatrix(1, 0) + ALY * (I01 * avargeY + I02 * slope[1]);
                        loadMatrix(5, 0) = OMALY * loadMatrix(5, 0) + ALY * (I02 * avargeY - I41 * slope[1] * DT) * 0.5;
                        loadMatrix(7, 0) = OMALY * loadMatrix(7, 0) + ALY * (length * S2 * avargeY + S3 * slope[1] * DT);
                        loadMatrix(11, 0) = OMALY * loadMatrix(11, 0) - ALY * (I02 * avargeY - I41 * slope[1] * DT) * 0.5;
                    }

                    if (ALZ > TOLERANCE)
                    {
                        const auto& avargeZ = avage[2];
                        loadMatrix(2, 0) = OMALZ * loadMatrix(2, 0) + ALZ * (I01 * avargeZ + I02 * slope[2]);
                        loadMatrix(4, 0) = OMALZ * loadMatrix(4, 0) - ALZ * (I02 * avargeZ - I41 * slope[2] * DT) * 0.5;
                        loadMatrix(8, 0) = OMALZ * loadMatrix(8, 0) + ALZ * (length * S2 * avargeZ + S3 * slope[2] * DT);
                        loadMatrix(10, 0) = OMALZ * loadMatrix(10, 0) + ALZ * (I02 * avargeZ - I41 * slope[2] * DT) * 0.5;
                    }

                }
                else
                {
                    // 荷载类型为moment
                    I01 = length * (S1 - S2);
                    I11 = -6.0 * (S2 - S3);
                    I21 = -I11;
                    I31 = S1 - 4.0*S2 + 3.0*S3;
                    I41 = -2.0*S2 + 3.0*S3;

                    if (fabs(pLoad1.m_p1 - pLoad1.m_p2) > TOLERANCE)
                    {
                        // 线性分布moment荷载
                        I02 = (S2 - S3) * length * length;
                        I12 = -6.0 * length *(S3 - S4);
                        I22 = -I12;
                        I32 = length * (S2 - 4.0*S3 + 3.0*S4);
                        I42 = -length * (2.0*S3 - 3.0*S4);
                        DT  = (x2 - x1)*length;
                        slope[0] = (load(3,0) - load(0,0)) / DT;
                        slope[1] = (load(5,0) - load(2,0)) / DT;
                        slope[2] = -(load(4,0) - load(1,0)) / DT;  
                        avage[0] =  load(0,0) - x1*slope[0]*length;    
                        avage[1] =  load(2,0) - x1*slope[1]*length;    
                        avage[2] = -load(1,0) - x1*slope[2]*length;   
                    }
                    else
                    {
                        // 均布moment荷载
                        avage[0] = load(0,0);
                        avage[1] = load(2,0);
                        avage[2] = -load(1,0);
                    }

                    // 计算等效节点荷载
                    std::array<Real, 5> I1 = {I01, I11, I21, I31, I41};
                    std::array<Real, 5> I2 = {I02, I12, I22, I32, I42};
                    CalculateLoadMatrix(I1, I2, avage, slope, length, S2, S3, loadMatrix);
                    
                    // 剪切修正
                    std::swap(loadMatrix(0, 0), loadMatrix(3, 0));
                    std::swap(loadMatrix(6, 0), loadMatrix(9, 0));
                    DT = length * length;
                    if (ALY > TOLERANCE)
                    {
                        const auto& slopeY = slope[1];
                        const auto& avargeY = avage[1];
                        loadMatrix(1, 0) = OMALY * loadMatrix(1, 0);
                        loadMatrix(5, 0) = OMALY * loadMatrix(5, 0) + ALY * (I01 * avargeY + I02 * slopeY);
                        loadMatrix(7, 0) = OMALY * loadMatrix(7, 0);
                        loadMatrix(11, 0) = OMALY * loadMatrix(11, 0) + ALY * (length * S2 * avargeY + S3 * slopeY * DT);
                    }
                    
                    if (ALZ > TOLERANCE)
                    {
                        const auto& slopeZ = slope[2];
                        const auto& avargeZ = avage[2];
                        loadMatrix(2, 0) = OMALZ * loadMatrix(2, 0);
                        loadMatrix(4, 0) = OMALZ * loadMatrix(4, 0) - ALZ * (I01 * avargeZ + I02 * slopeZ); 
                        loadMatrix(8, 0) = OMALZ * loadMatrix(8, 0);
                        loadMatrix(10, 0) = OMALZ * loadMatrix(10, 0) - ALZ * (length * S2 * avargeZ + S3 * slopeZ * DT); 
                    }                   
                }
            }
            else
            {   
                if (pLoad1.isForce())
                {
                    // 荷载类型为集中FORCE
                    calcConcentratedForce(x1, length, load, OMALY, OMALZ, _OUT loadMatrix);
                }
                else
                {
                    // 荷载类型为集中MOMENT
                    calcConcentratedMoment(x1, length, load, OMALY, OMALZ, _OUT loadMatrix);
                }
            }

            // PIN处理, 释放自由度。
            auto dofsPaRelease = barData.GetDofSetOfPaRelease();        //左端自由度释放集
            auto dofsPbRelease = barData.GetDofSetOfPbRelease();        //右端自由度释放集
            if (!(dofsPaRelease.empty() and dofsPaRelease.empty()))
            {
                Matrixd     KE;                                             //单元刚度矩阵      
                BarCalculator::ComputeStiffness(length, material, property, _OUT KE);
                BarCalculator::ReleaseLoadDof(dofsPaRelease, dofsPbRelease, KE, _OUT loadMatrix);  //释放等效节点荷载自由度
            }

            // 转化为global坐标
            loadMatrix = TEle.Transpose () * loadMatrix;
        }   
        break;

        // case ElementTypes::ELETYPE_Beam:
        // {        
        //     CBEAMData   BeamData;
        //     if(pLoad1.m_x1 == pLoad1.m_x2 || pLoad1.m_x2 == 0.0)
        //     {
        //     }
        //     else
        //     {
    
        //     }
        // }
        // break;

        //case ElementTypes::ELETYPE_Bend:
        //break;
        default:
        {
            SGError << "PLoad1 can't deal with the element, which id is " << pLoad1.m_eId;
            break;
        }
    }
        
}

void LoadCalculator::PLoad2Calc (const SG::DBManager::DBServiceFactorySP   dbServiceFactory,
                                               const SG::DataStructure::FEM::PLOAD2Data& pLoad2,
                                               SG::Algebra::Matrix& _OUT                 loadMatrix,
                                               std::vector<std::size_t>& _OUT            nodeIndexes)
{
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    IdArray     nodelist       = dbServiceFactory->get<IElementService> ()->FindElementNodes (pLoad2.m_eId);
    const auto  nNode          = nodelist.size ();
    PLOADData   pLoad;
    pLoad.m_P = pLoad2.m_P;
    for (std::size_t index = 0; index != nNode; ++index)
    {
        pLoad.m_G[index] = nodelist[index];
    }
    auto& assembly = SG::FEMSolver::AssemblyEigen::GetInstance ();
    nodeIndexes.resize (nNode);
    assembly.FindNodeIndex (pLoad.m_G, _OUT nodeIndexes);

    LoadCalculator::PLoadCalc (nodeRepository, pLoad, loadMatrix);
}

/** @brief   计算PLoad4载荷方向
 * 
 * 当pload4数据中定义方向的矢量非零, 当PLoad4使用该矢量方向；否则载荷使用作用面面的法线方向
 * @param[in][out] 输入为面法线，输出为载荷方向
 * @author   Yuan Chenxi 
 * @date     2025.02.18
 */
void DirectionModify (const SG::DataStructure::FEM::PLOAD4Data& pLoad4, Vector3D& _OUT normal)
{
    Vector3D n = pLoad4.GetDirection ();
    if(!n.IsZero ())
    {
        normal = n;
        if (0 == pLoad4.m_cId)
        {
            // 对 n 进行坐标变换  
            const auto& coordProcessor = CoordinateProcessor::GetInstance ();
            coordProcessor.Rotate2Basic (pLoad4.m_cId, n);
        }
        normal.Normalize();
    }
}

/** @brief   修改面中节点顺序，使得G1成为面的第一个节点。此操作不会改变面的法线 
 * 
 * @param    posiG1         节点在母元中索引编号
 * @param    nodelist       单元节点全局编号
 * @param    surfNodelist   母元中面节点索引
 * @param[out]    NewSurfNodelist 修改顺序的索引
 * @author   Yuan Chenxi
 * @date     2025.02.18
 */
void SurfNodelistModify (const int& posiG1, const IdArray& nodelist, const std::vector<int>& surfNodelist, IdArray& _OUT NewSurfNodelist)
{
    NewSurfNodelist.clear();
    auto it = std::find(surfNodelist.begin(), surfNodelist.end(), posiG1);
    while(NewSurfNodelist.size() < surfNodelist.size())
    {
        if (it == surfNodelist.end())
        {
            it = surfNodelist.begin();
        }
        NewSurfNodelist.push_back(nodelist[*it]);
        ++it;
    }
}

/** @brief    找到G1和G34在母元中的节点索引位置
 * 
 * @param    G1 
 * @param    G34
 * @param    nodelist 
 * @param[out]    posiG1 
 * @param[out]    posiG34 
 * @return   
 * @author   Yuan Chenxi
 * @date     2025.02.20
 */
static bool FindG1G34Posi (Id G1, Id G34, const IdArray& eleNodes, int& _OUT posiG1, int& _OUT posiG34) 
{
    posiG1 = -1;
    posiG34 = -1;
    for(std::size_t i = 0; i < eleNodes.size (); ++i)
    {
        if(eleNodes[i] == G1)
        {
            posiG1 = i;
        }
        else if(eleNodes[i] == G34)
        {
            posiG34 = i;
        }
    }

    bool findNode = (-1 != posiG1)&&(-1 != posiG34);
    return findNode;
}

Status LoadCalculator::PLoad4Calc (const SG::DBManager::DBServiceFactorySP&  dbServiceFactory,
                                               const SG::DataStructure::FEM::PLOAD4Data& pLoad4,
                                               SG::Algebra::Matrix& _OUT                 loadMatrix,
                                               std::vector<std::size_t>& _OUT            nodeIndexes)
{
    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();
    const auto& eleService = dbServiceFactory->get<IElementService> ();
    const auto& coordProcessor = CoordinateProcessor::GetInstance ();
    
    // PLOAD4作用的单元的类型
    SG::DataStructure::FEM::ElementTypes eleType = eleService->GetElementType (pLoad4.m_eId);
    if (ElementTypes::ELETYPE_Unknown == eleType)
    {
        SGWarn << "PLoad4 can't find element, with id = " << pLoad4.m_eId;
        return Status::STATUS_Failure;
    }

    IdArray nodelist = eleService->FindElementNodes (pLoad4.m_eId);

    /** 根据单元类型不同，找到对应的载荷作用面节点在Basic坐标系下坐标， 节点的全局索引编号nodeIndexes 以及位移输出坐标系 dispCoordIds
     */
    int nNodes = 0;
    nodeIndexes.clear();
    PointArray points;
    IdArray    dispCoordIds;
    const bool isSolid = (pLoad4.m_G1 > 0);
    if (false == isSolid)
    {
        switch (eleType)
        {
            case ElementTypes::ELETYPE_ShellTria3: 
            {
                // 获取节点坐标信息
                nNodes = 3;
                break;
            }
            case ElementTypes::ELETYPE_ShellQuad4: 
            {
                // 获取单元坐标信息
                nNodes = 4;
                break;
            }
            default:
            SGError << "PLoad4 Can't support this kind of shell element";
            break;
        }
        ModelInfoQuery::QueryNode (nodeRepository, nodelist.data(), nNodes, points, nodeIndexes, dispCoordIds);
    }
    else
    {
        int posiG1;
        int posiG34;
        const auto status = FindG1G34Posi (pLoad4.m_G1, pLoad4.m_G34, nodelist, _OUT posiG1, _OUT posiG34);
        if (!(status || (ElementTypes::ELETYPE_Penta6SRI == eleType && posiG1 > -1)))
        {
            SGError << "PLoad4 Info Error";
            return Status::STATUS_Failure;
        }

        //根据pLoad4的成员G1和G34找到载荷作用面 {G1 和 G34 两个节点同时和某个面匹配，才能够确定当前面位置}
        auto FindSurf = [&posiG1, &posiG34, &nodelist] (const std::vector<std::vector<int>>& surfaces, IdArray& _OUT surfNodelist) -> void 
        {
            //所有构成四边形的截面的节点在列表中的位置有两个与G1和G34在列表中的位置相同，则找到了对应的四边形作用面
            for(auto& surf : surfaces)
            {
                int sameId = 0;
                for(const auto& posi : surf)
                {
                    // 当前面包含 节点G1 和 G34
                    if (posi == posiG1 || posi == posiG34)
                    {
                        ++sameId;
                    }
                }

                if(sameId == 2)
                {
                    // surf 为要寻找的平面
                    SurfNodelistModify(posiG1, nodelist, surf, surfNodelist);
                    return;
                }
            }

            SGError << "can't search the surface applying load.  \n";
        };

        // find the surface applyed by load
        IdArray surfNodelist;
        switch (eleType)
        {
            case SG::DataStructure::FEM::ElementTypes::ELETYPE_Hexa8SRI:
            {
                nNodes = 4;
                // static std::vector<std::vector<int>> surfaces = {{0, 1, 2, 3}, {4, 7, 6, 5}, {1, 5, 6, 2}, {4, 0, 3, 7}, {0, 4, 5, 1}, {6, 7, 3, 2}};
                static std::vector<std::vector<int>> surfaces = {{0, 1, 2, 3}, {4, 7, 6, 5}, {1, 5, 2, 6}, {4, 0, 3, 7}, {0, 4, 5, 1}, {6, 2, 3, 7}};
                FindSurf(surfaces, _OUT surfNodelist);
                break;
            }
            case SG::DataStructure::FEM::ElementTypes::ELETYPE_Pyram5R:
            {
                // 如果G1和G34相差2，作用面为底面四边形; 否则作用面是侧面三角形

                if(abs(posiG1 - posiG34) == 2)
                {
                    nNodes = 4;
                    surfNodelist.resize(nNodes);
                    static std::vector<int> surface = {0, 1, 2, 3};
                    SurfNodelistModify(posiG1, nodelist, surface, surfNodelist);
                }
                else
                {
                    nNodes = 3;
                    // 获取截面,截面节点。给三角形面节点排序，节点在列表中的位置相差1的时候，让较小的位置放在第一个点，金字塔顶点为第二个点，较大的为第三个点时，载荷朝向面内，与Nastran一致。
                    // 位置相差3的时候，让较小的位置放在第三个点，金字塔顶点为第二个点，较大的为第一个点时，载荷朝向面内。
                    static std::vector<std::vector<int>> surfaces = {{0, 4, 1}, {3, 4, 0}, {3, 2, 4}, {1, 4, 2}};
                    FindSurf(surfaces, surfNodelist);
                }
                break;
            }    
            case SG::DataStructure::FEM::ElementTypes::ELETYPE_Penta6SRI:
            {
                /** 载荷作用在三角形还是四边形面？ G3字段为空，表示上下两个三角形面
                 */ 
                if (pLoad4.m_G34 == 0)     
                {
                    nNodes = 3;
                    surfNodelist.resize(3);
                    // 判断载荷是作用在上表面还是下表面
                    if (posiG1 < 3)
                    {
                        std::vector<int> triFace = {0, 1, 2};
                        SurfNodelistModify (posiG1, nodelist, triFace, surfNodelist);
                    }
                    else
                    {
                        std::vector<int> triFace = {3, 5, 4};
                        SurfNodelistModify (posiG1, nodelist, triFace, surfNodelist);
                    }
                }
                else 
                {
                    nNodes = 4;
                    static std::vector<std::vector<int>> surfaces = {{0, 2, 5, 3}, {1, 0, 3, 4}, {1, 4, 5, 2}};
                    FindSurf(surfaces, _OUT surfNodelist);
                }
                break;
            }
            case SG::DataStructure::FEM::ElementTypes::ELETYPE_Tetra4:
            {
                nNodes = 3;
                // G4节点不在载荷作用面上，因此直接找到载荷作用面
                static std::vector<std::vector<int>> surfaces = {{1, 3, 2}, {0, 2, 3}, {0, 3, 1}, {0, 1, 2}};
                const auto& surf = surfaces[posiG34];
                // 根据 G1 进行调整
                SurfNodelistModify(posiG1, nodelist, surf, surfNodelist);
                break;
            }
            default:
                break;
        }
        
        // 通过载荷作用节点的id `surfNodelist` 读取节点坐标
        ModelInfoQuery::QueryNode (nodeRepository, surfNodelist.data(), nNodes, points, nodeIndexes, dispCoordIds);
    }

    // 积分计算面载荷的等效节点力
    if (3 == nNodes)
    {
        //计算载荷方向,顺便可以输出三角形面积
        Vector3D n;
        auto area = calcTriArea (points[0], points[1], points[2], n);
        DirectionModify(pLoad4, n);

        Real pressure[3]{pLoad4.m_P[0], pLoad4.m_P[1], pLoad4.m_P[2]};
        /** @brief    二维单元面和三维单元面法线不同。
         *           三维单元面片，向积分点插值时，节点压力数值 P1-P4（P3）需要重新排序; 使得形函数和面片的局部坐标系一致
         */
        if(isSolid)
        {
            pressure[1] = pLoad4.m_P[2];
            pressure[2] = pLoad4.m_P[1];
        }

        // 计算平面三节点三角形表面载荷向量
        auto Pload4TriCalc = [&pressure] (const Vector3D& n, const Real& area, Matrix& _OUT loadMatrix) -> void 
        {   
            loadMatrix = Matrix(9, 1); 
            
            Real N[3];
            Gauss2DTria3P IntegPointsSet;
            for (std::size_t i = 0; i < IntegPointsSet.GetPointSize (); ++i)
            {
                // 插值得到积分点处的载荷向量
                N[0] = IntegPointsSet.GetPoint (i).m_x;
                N[1] = IntegPointsSet.GetPoint (i).m_y;
                N[2] = 1 - N[0] - N[1];
                const auto pInteg = (N[0]*pressure[0]) + (N[1]*pressure[1]) + (N[2]*pressure[2]);

                // 载荷贡献分配到节点上
                for (auto iNode = 0; iNode < 3; ++iNode)
                {
                    const auto Fi = N[iNode] * area * pInteg * IntegPointsSet.GetPoint (i).m_weight;
                    loadMatrix(iNode*3, 0)   += Fi * n.m_x;
                    loadMatrix(iNode*3+1, 0) += Fi * n.m_y;
                    loadMatrix(iNode*3+2, 0) += Fi * n.m_z;
                }
            }
        };

        Pload4TriCalc (n, area, loadMatrix);
    }
    else if (4 == nNodes)
    {
        Matrix T(3, 3);
        Matrix localCoords (4, 2);
        Point origin;
        ElementTools::computeQaud4CoordTrans(points, T, origin, localCoords);

        // 计算面法线, 变换到局部坐标系
        Vector3D n{T(0, 2), T(1, 2), T(2, 2)};
        DirectionModify(pLoad4, n);
        
        // 计算平面四节点四边形表面载荷向量
        Real pressure[4]{pLoad4.m_P[0], pLoad4.m_P[1], pLoad4.m_P[2], pLoad4.m_P[3]};
        if(isSolid)
        {
            pressure[1] = pLoad4.m_P[3];
            pressure[3] = pLoad4.m_P[1];
        }

        auto PloadQuadCalc = [&pressure] (const Vector3D& n, const Matrix& localCoords, Matrix& _OUT loadMatrixBasic) -> void 
        {
            //计算积分点处的载荷向量
            Gauss2D2X2P IntegPointsSet;
            Plane4NodeShpFunc shpFunc;
            shpFunc.Compute (IntegPointsSet.GetPoints());

            loadMatrixBasic = Matrix(12, 1);

            for (std::size_t i = 0; i != IntegPointsSet.GetPointSize (); ++i)
            {
                // 通过形函数插值，计算积分点压强
                const auto& NG = shpFunc.GetShapeFunction(i);
                Real pGauss[3]{0, 0, 0};
                for (auto j = 0; j != 4; ++j)
                {
                    auto Pi = NG(j, 0) * pressure[j];

                    pGauss[0] += n.m_x * Pi;
                    pGauss[1] += n.m_y * Pi;
                    pGauss[2] += n.m_z * Pi;
                }

                //计算雅可比矩阵行列式和形函数矩阵N
                const auto& DNDxi = shpFunc.GetShapeFunctionDerivative(i);
                auto jacobi = DNDxi * localCoords;
                const auto jw = det2X2 (jacobi) * IntegPointsSet.GetPoint (i).m_weight;
                
                // 载荷贡献分配到节点上
                for (auto iNode = 0; iNode < 4; ++iNode)
                {
                    const auto Fi = NG(iNode, 0) * jw;
                    loadMatrixBasic(iNode*3, 0)   += Fi * pGauss[0];
                    loadMatrixBasic(iNode*3+1, 0) += Fi * pGauss[1];
                    loadMatrixBasic(iNode*3+2, 0) += Fi * pGauss[2];
                }
            }
        };

        PloadQuadCalc (n, localCoords, loadMatrix);
    }

    // 载荷转换到位移坐标系函数
    for (size_t i = 0; i < points.size(); ++i)
    {
        if (0 != dispCoordIds[i])
        {
            Matrix Tg (3, 3);
            coordProcessor.GetCoordTrans (dispCoordIds[i], points[i], _OUT Tg);
            Matrix pointLoad{3, 1, {loadMatrix(i * 3, 0), loadMatrix(i * 3 + 1, 0), loadMatrix(i * 3 + 2, 0)}};
            pointLoad = Tg * pointLoad;
            for(auto j = 0; j < 3; ++j)
            {
                loadMatrix(i * 3 + j, 0) = pointLoad(j, 0);
            }
        }
    }

    return Status::STATUS_Success;
}


void LoadCalculator::GravityLoadCalc(const SG::DBManager::DBServiceFactorySP&             dbServiceFactory,
                                     const std::set<SG::DataStructure::Common::Id>&       loadCaseIds)
{
    const auto GRAVDatas = dbServiceFactory->get<ILoadSummaryService> ()->FindGRAV (loadCaseIds);
    const auto nGrav = GRAVDatas.size();
    if (0 == nGrav)
    {
        return;
    }
    auto& assembly = AssemblyEigen::GetInstance ();
    const auto& coordProcessor = CoordinateProcessor::GetInstance();

    auto nodeService = dbServiceFactory->get<INodeService>();
    if (!nodeService) { SGError << "获取INodeService失败"; return; }


    // 1. 获取Basic坐标系下的重力加速度集
    IdArray sIdSet;                  // 重力荷载ID集合
    std::vector<Vector3D> gravAccSet; // Basic坐标系下的重力加速度向量（x,y,z分量）
    SG::Algebra::Matrixd T1(3, 3);
    for (const auto& gravity : GRAVDatas) 
    {
        const auto& sid = gravity.m_sId;
        SG::Algebra::Vector3D load0{gravity.m_N[0] * gravity.m_a, gravity.m_N[1] * gravity.m_a, gravity.m_N[2] * gravity.m_a};
        
        const auto cId = gravity.m_cId;
        if (0 != cId) 
        {
            const auto coordP = coordProcessor.GetCoord(cId);
            coordProcessor.GetCoordTrans(cId, coordP->GetOrigin(), _OUT T1);
            load0 = load0.Rotate(T1);
        }

        sIdSet.push_back(sid);
        gravAccSet.push_back(load0);
    }

    // 获取质量矩阵
    const EigenSparse& CoupledMTotal = assembly.GetCoupledMassMTotal();
    
    // 初始化每个加速度集对应的全局加速度向量
    const int totalDofs = static_cast<int>(CoupledMTotal.rows());  // 整体自由度数量（与质量矩阵维度一致）
    std::vector<EigenVector> globalAccVectors (nGrav, EigenVector::Zero(totalDofs));
    const DofSet possibleDofs{ DOFDirections::DOF_Ux, DOFDirections::DOF_Uy, DOFDirections::DOF_Uz }; // 定义节点可能包含的自由度类型（仅考虑平动）
            
    const auto& nodeIds = assembly.GetNodeIds(); // 所有节点ID
    const auto nNodes = nodeIds.size();              // 节点数
    NodeData node;  // 节点信息
        
    std::vector<int> dofGlobalIds;
    Point nodePoint;
    Matrixd T(3, 3); // 坐标转换矩阵

    // 2. 循环所有节点
    for (std::size_t nodeIdx = 0; nodeIdx < nNodes; ++nodeIdx) 
    {
        // 获取节点编号&线自由度全局编号
        const Id nodeId = nodeIds[nodeIdx];
        if ( (nodeId <= 0) or (nodeId >= VIRTUAL_GRID_ID_ORIGIN)) // 判断虚节点
        {
            SGWarn << "无效节点ID" << nodeId << "跳过重力荷载计算";
            continue;
        }
        assembly.GetGlobalDOFId({nodeIdx}, possibleDofs, _OUT dofGlobalIds);
        
        // 获取节点位移输出坐标系
        if (Status::STATUS_Success != nodeService->FindByIndex (nodeIdx, _OUT node))
        {
            SGWarn << "节点ID" << nodeId << "查找失败，跳过重力荷载计算";
            continue;
        }

        auto dispCoordId = node.m_dispCoord;
        if (dispCoordId > 0)
        {
            coordProcessor.GetGlobalCoordinate(node, _OUT nodePoint, _OUT dispCoordId);
            coordProcessor.GetCoordTrans(dispCoordId, nodePoint, _OUT T);
        }

        // 2.2 循环每个加速度集，计算当前节点对该加速度集的贡献
        for (std::size_t iGrav = 0; iGrav < nGrav; ++iGrav)
        {
            auto globalGravAcc = gravAccSet[iGrav];
            // 节点加速度坐标系转换：Basic到Global
            if (dispCoordId > 0)
            {
                globalGravAcc = globalGravAcc.Rotate(T);
            }

            if (dofGlobalIds[0]>= 0 && dofGlobalIds[0] < totalDofs)
            {
                globalAccVectors[iGrav](dofGlobalIds[0]) = globalGravAcc.m_x;
            }
            if (dofGlobalIds[1]>= 0 && dofGlobalIds[1] < totalDofs)
            {
                globalAccVectors[iGrav](dofGlobalIds[1]) = globalGravAcc.m_y;
            }
            if (dofGlobalIds[2]>= 0 && dofGlobalIds[2] < totalDofs)
            {
                globalAccVectors[iGrav](dofGlobalIds[2]) = globalGravAcc.m_z;
            }
        }
    }

    // 3. 循环每个加速度集，计算整体重力荷载：[F] = [M] * [a]
    for (std::size_t iGrav = 0; iGrav < nGrav; ++iGrav)
    {
        const EigenVector& globalAccVector = globalAccVectors[iGrav];
        const EigenVector globalGravLoad = CoupledMTotal * globalAccVector;
        
        assembly.AccumulateLoad(sIdSet[iGrav], globalGravLoad);
    } 
}

void LoadCalculator::calcDAREAForce (const std::vector<SG::DataStructure::FEM::DAREAData>& DAREAList, SG::Algebra::Vec_t& _OUT force)
{
    auto&       assembly = SG::FEMSolver::AssemblyEigen::GetInstance ();
    std::size_t nTotalDOF       = assembly.GetTotalDOFSize ();
    force            = SG::Algebra::Vec_t (nTotalDOF);
    force.setZero ();
    
    for (auto&& darea : DAREAList)
    {
        std::size_t nodeIndex;
        int         dofGlobalIds;
        assembly.FindNodeIndex (darea.m_P, _OUT nodeIndex);
        assembly.GetGlobalDOFId (nodeIndex, darea.GetDirection (), _OUT dofGlobalIds);
        force[dofGlobalIds] += darea.m_A;
    }

    // 约束处理
    assembly.ProcessLoadConstraint(_OUT force);
}

void LoadCalculator::ComputeStaticLoad (const DBServiceFactorySP dbServiceFactory, const std::set<SG::DataStructure::Common::Id>& loadCaseIds)
{
    auto& assembly = AssemblyEigen::GetInstance ();
    auto& coordHandler = CoordinateProcessor::GetInstance();
    static const DofSet transDofs =  {
        DOFDirections::DOF_Ux, 
        DOFDirections::DOF_Uy,   
        DOFDirections::DOF_Uz};
        
    static const DofSet rotDofs =  {DOFDirections::DOF_Rotx, 
                                    DOFDirections::DOF_Roty,   
                                    DOFDirections::DOF_Rotz};

    static const DofSet fullDofs = {transDofs[0], transDofs[1], transDofs[2],  // 平动自由度（Ux, Uy, Uz）
                                    rotDofs[0],   rotDofs[1],   rotDofs[2]     // 转动自由度（Rotx, Roty, Rotz）
                                   };

    const auto& nodeRepository = dbServiceFactory->get<INodeService> ();

    // Force2 和 Moment2 载荷计算
    auto Force2Calc = [&nodeRepository, &assembly, &coordHandler](const FORCE2Data& force, SG::DataStructure::Common::Real mod, std::size_t applynodeIndex) -> Matrix
    {
        // 节点在世界坐标系下坐标值
        PointArray pointArray(4);
        
        std::size_t nodeIndex;
        Id tmpDispCoordId; // 不需要返回
        NodeData node;
        for (int iNode = 0; iNode < 4; ++iNode)
        {
            assembly.FindNodeIndex (force.m_gi[iNode], _OUT nodeIndex);
            nodeRepository->FindByIndex (nodeIndex, _OUT node);
            coordHandler.GetGlobalCoordinate (node, _OUT pointArray[iNode], _OUT tmpDispCoordId);
        }

        // G1G2, G3G4叉乘确定载荷方向
        const Vector3D direction1 = SG::Algebra::Distance(pointArray[1], pointArray[0]);
        const Vector3D direction2 = SG::Algebra::Distance(pointArray[3], pointArray[2]);
        if (direction1.IsZero() || direction2.IsZero())
        {
            SGError << "Force2Calc can't calc direction";
            return Matrix();
        }

        Vector3D n = Cross(direction1, direction2);
        // 还需要检查共线导致 n为 零向量
        Real norm;
        n.Normalize(norm);

        // translate to the analysis coordinate of load applying node
        nodeRepository->FindByIndex (applynodeIndex, _OUT node);
        const auto& dispCoordId = node.m_dispCoord;
        if (0 != dispCoordId)
        {
            Matrixd TG(3, 3);
            Point point;
            coordHandler.GetGlobalCoordinate (node, _OUT point, _OUT tmpDispCoordId);
            coordHandler.GetCoordTrans (dispCoordId, point, _OUT TG);
            n = n.Rotate (TG);
        }

        return Matrix (3, 1, {n.m_x*mod, n.m_y*mod, n.m_z*mod});
    };

    auto Force1Calc = [&nodeRepository, &assembly, &coordHandler] (const FORCE1Data& force, std::size_t applynodeIndex) ->  Matrix
    {
        // 计算载荷方向
        Point G1, G2;
        NodeData node;
        Id  tmpDispCoordId;
        std::size_t nodeIndex;
        assembly.FindNodeIndex (force.m_gi[0], _OUT nodeIndex);
        nodeRepository->FindByIndex(nodeIndex, _OUT node);
        coordHandler.GetGlobalCoordinate (node, _OUT G1, _OUT tmpDispCoordId);
        assembly.FindNodeIndex (force.m_gi[1], _OUT nodeIndex);
        nodeRepository->FindByIndex(nodeIndex, _OUT node);
        coordHandler.GetGlobalCoordinate (node, _OUT G2, _OUT tmpDispCoordId);

        // G1指向G2确定载荷方向
        Vector3D direction = SG::Algebra::Distance(G2, G1);
        if (direction.IsZero())
        {
            SGError << "Force1Calc G1 & G2 coniside ";
            return Matrix();
        }
        Real norm;
        direction.Normalize(norm);

        // translate to the analysis coordinate of load applying node
        nodeRepository->FindByIndex (applynodeIndex, _OUT node);
        const auto& dispCoordId = node.m_dispCoord;
        if (0 != dispCoordId)
        {
            Matrixd TG(3, 3);
            Point point;
            coordHandler.GetGlobalCoordinate (node, _OUT point, _OUT tmpDispCoordId);
            coordHandler.GetCoordTrans (dispCoordId, point, _OUT TG);
            direction = direction.Rotate (TG);
        }
        
        return Matrix (3, 1, {direction.m_x*force.m_force, direction.m_y*force.m_force, direction.m_z*force.m_force});
    };

    // 右端项增加节点力的贡献
    SG::DataStructure::FEM::LoadTypeSet LoadTypes{
        LoadType::LOAD_ConcentratedForce,
        LoadType::LOAD_ConcentratedMoment,
        LoadType::LOAD_Force1,
        LoadType::LOAD_Moment1,
        LoadType::LOAD_Force2,
        LoadType::LOAD_Moment2,
        LoadType::LOAD_SLOAD,
        LoadType::LOAD_PLOAD,
        LoadType::LOAD_PLOAD1,
        LoadType::LOAD_PLOAD2,
        LoadType::LOAD_PLOAD4,
        LoadType::LOAD_QBDY3
    };
    // dbServiceFactory -> GetActiveLoadTypes (loadCaseId, _OUT LoadTypes);

    std::vector<int> dofIds(3);
    std::size_t nodeIndex;
    std::vector<std::size_t> nodeIndexes(4);
    // Matrix loadVector(3, 1);
    Matrix loadMatrix;

    static const DofSet temperatureDofs = {
        DOFDirections::DOF_Ux
    };
    std::vector<int> tempdofIds(1);
    Matrix temploadVector(1, 1);
    for (const auto& loadCaseId : loadCaseIds)
    {
        std::cout << "== compute ["<< loadCaseId << "]Load Vector start \n";
        for (auto& type : LoadTypes)
        {
            switch(type)
            {
                case LoadType::LOAD_ConcentratedForce:
                { 
                    FORCEDataArray forceSet;
                    dbServiceFactory->get<IFORCEService> () -> FindBySId (loadCaseId, forceSet);
                    for (const auto& force : forceSet)
                    {
                        // 载荷作用的全局自由度
                        assembly.ComputeGlobalDOFId (force.m_nodeId, transDofs, _OUT dofIds);

                        // basic 坐标系下坐标
                        std::size_t nodeIndex;
                        assembly.FindNodeIndex (force.m_nodeId, _OUT nodeIndex);
                        NodeData node;
                        nodeRepository->FindByIndex (nodeIndex, _OUT node);

                        LoadCalculator::ForceCalc(coordHandler, force, node, _OUT loadMatrix);
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << force.m_nodeId;}
                    }
                    break;
                }
                case LoadType::LOAD_ConcentratedMoment:
                {
                    MOMENTDataArray moments;
                    dbServiceFactory->get<IMOMENTService> () -> FindBySId (loadCaseId, moments);
                    for (const auto& momnet : moments)
                    {
                        assembly.ComputeGlobalDOFId (momnet.m_nodeId, rotDofs, _OUT dofIds);

                        std::size_t nodeIndex;
                        assembly.FindNodeIndex (momnet.m_nodeId, _OUT nodeIndex);
                        NodeData node;
                        nodeRepository->FindByIndex (nodeIndex, _OUT node);

                        LoadCalculator::ForceCalc(coordHandler, momnet, node, _OUT loadMatrix);
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << momnet.m_nodeId;}
                    }         
                    break;
                }
                case LoadType::LOAD_Force1:
                { 
                    FORCE1Array force1Set;
                    dbServiceFactory->get<IFORCE1Service> () -> FindBySId (loadCaseId, force1Set);
                    for (const auto& force: force1Set)
                    {
                        // 载荷作用的全局自由度编号
                        assembly.ComputeGlobalDOFId (force.m_nodeId, transDofs, _OUT dofIds);
                        
                        assembly.FindNodeIndex (force.m_nodeId, _OUT nodeIndex);
                        loadMatrix = Force1Calc (force, nodeIndex);
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << force.m_nodeId;}
                    }
                    break;
                }
                case LoadType::LOAD_Moment1:
                {
                    MOMENT1Array moment1Set;
                    dbServiceFactory->get<IMOMENT1Service> () -> FindBySId (loadCaseId, moment1Set);        
                    for (const auto& force: moment1Set)
                    {
                        // 载荷作用的全局自由度编号
                        assembly.ComputeGlobalDOFId (force.m_nodeId, rotDofs, _OUT dofIds);

                        assembly.FindNodeIndex (force.m_nodeId, _OUT nodeIndex);
                        loadMatrix = Force1Calc (force, nodeIndex);
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << force.m_nodeId;}
                    }
                    break;
                }
                case LoadType::LOAD_Force2:
                { 
                    FORCE2DataArray force2Set;
                    dbServiceFactory->get<IFORCE2Service> () -> FindBySId (loadCaseId, force2Set);
                    for (const auto& force: force2Set)
                    {
                        // 载荷作用的全局自由度编号
                        assembly.ComputeGlobalDOFId (force.m_nodeId, transDofs, _OUT dofIds);

                        assembly.FindNodeIndex (force.m_nodeId, _OUT nodeIndex);
                        loadMatrix = Force2Calc (force, force.m_force, nodeIndex);
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << force.m_nodeId;}
                    }
                    break;
                }
                case LoadType::LOAD_Moment2:
                { 
                    MOMENT2Array moment2Set;
                    dbServiceFactory->get<IMOMENT2Service> () -> FindBySId (loadCaseId, moment2Set);
                    for (const auto& force: moment2Set)
                    {
                        // 载荷作用的全局自由度编号
                        assembly.ComputeGlobalDOFId (force.m_nodeId, rotDofs, _OUT dofIds);

                        assembly.FindNodeIndex (force.m_nodeId, _OUT nodeIndex);
                        loadMatrix = Force2Calc(force, force.m_force, nodeIndex);
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << force.m_nodeId;}
                    }
                    break;
                }   
                case LoadType::LOAD_SLOAD:  
                { 
                    std::vector<SG::DataStructure::FEM::SLOADData> forceSet;
                    dbServiceFactory->get<ISLOADService> () -> FindBySId (loadCaseId, _OUT forceSet);
                    for (const auto& force : forceSet)
                    {
                        // 载荷作用的全局自由度
                        assembly.ComputeGlobalDOFId (force.m_G, temperatureDofs, _OUT tempdofIds);

                        //获取荷载值
                        temploadVector(0,0) = force.m_F;
                        auto status = assembly.AccumulateLoad (loadCaseId, tempdofIds, temploadVector);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply force to load. which id is " << force.m_G;}
                    }
                    
                    break; 
                }
                case LoadType::LOAD_QBDY3:
                {
                    std::vector<SG::DataStructure::FEM::QBDY3Data> forceSet;
                    dbServiceFactory->get<IQBDY3Service> () -> FindBySId (loadCaseId, forceSet);
                    

                    const auto& elementService = dbServiceFactory->get<IElementService> ();
                    
                    CHBDYPData lineEle;
                    CHBDYGData surfaceEle;
                    for (const auto& force : forceSet)
                    {
                        const auto& q0 = force.m_Q0;
                        Id eleID = force.m_EID;
                        if (Status::STATUS_Success == elementService -> FindById(eleID, _OUT lineEle))
                        {
                            const auto& type = ELETYPE_CHBDYP;
                            SG::FEMSolver::Element::CHBDYPLINECalculator bdyeleCalculator;
                            bdyeleCalculator.Initialize (type);
                            bdyeleCalculator.ComputeHeatFluxMatries (dbServiceFactory, lineEle, q0, loadCaseId);                            
                        }
                        else if (Status::STATUS_Success == elementService -> FindById(eleID, _OUT surfaceEle))
                        {
                            const auto& type = ELETYPE_CHBDYG;
                            SG::FEMSolver::Element::CHBDYGAREACalculator bdyeleCalculator;
                            bdyeleCalculator.Initialize (type);
                            bdyeleCalculator.ComputeHeatFluxMatries (dbServiceFactory, surfaceEle, q0, loadCaseId);
                        }
                    }
                    
                    break; 
                }
                case LoadType::LOAD_PLOAD:
                {
                    PLOADArray ploadSet;
                    dbServiceFactory->get<IPLOADService> () -> FindBySId (loadCaseId, ploadSet);
                    for (const auto& force: ploadSet)
                    {
                        // triangle or quad ?
                        if(force.m_G[3] == 0)
                        {
                            nodeIndexes.resize(3);
                        }
                        else
                        {
                            nodeIndexes.resize(4);
                        }
                        assembly.FindNodeIndex (force.m_G, _OUT nodeIndexes);
                        assembly.GetGlobalDOFId  (nodeIndexes, transDofs, _OUT dofIds);
                        LoadCalculator::PLoadCalc (nodeRepository, force, loadMatrix);
                        assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                    }
                    break;
                } 
                case LoadType::LOAD_PLOAD1:
                {
                    PLOAD1Array pLoad1Set;
                    dbServiceFactory->get<IPLOAD1Service> () -> FindBySId (loadCaseId, pLoad1Set);
                    for (const auto& force: pLoad1Set)
                    {
                        std::vector<std::size_t> nodeIndexes;
                        LoadCalculator::PLoad1Calc(dbServiceFactory, force, loadMatrix, nodeIndexes);
                        assembly.GetGlobalDOFId  (nodeIndexes, fullDofs, _OUT dofIds);
                        std::cout << dofIds.size() << std::endl;
                        auto status = assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                        if (Status::STATUS_Failure == status) {SGError << "can't apply PLOAD1 to ele. which id is " << force.m_eId;}
                    } 
                    break;
                }
                case LoadType::LOAD_PLOAD2:
                {
                    PLOAD2Array pload2Set;
                    dbServiceFactory->get<IPLOAD2Service> () -> FindBySId (loadCaseId, pload2Set);
                    for (const auto& pload2: pload2Set)
                    {
                        std::vector<std::size_t> nodeIndexes;
                        LoadCalculator::PLoad2Calc(dbServiceFactory, pload2, _OUT loadMatrix, _OUT nodeIndexes);
                        assembly.GetGlobalDOFId  (nodeIndexes, transDofs, _OUT dofIds);
                        assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                    }
                    break;
                }
                case LoadType::LOAD_PLOAD4:
                {
                    PLOAD4Array pload4Set;
                    dbServiceFactory->get<IPLOAD4Service> () -> FindBySId (loadCaseId, pload4Set);
                    for (const auto& pload4: pload4Set)
                    {
                        std::vector<std::size_t> nodeIndexes;
                        const auto status = LoadCalculator::PLoad4Calc(dbServiceFactory, pload4, loadMatrix, nodeIndexes);
                        if (Status::STATUS_Failure == status)
                        {
                            continue;
                        }
                        
                        assembly.GetGlobalDOFId (nodeIndexes, transDofs, _OUT dofIds);
                        assembly.AccumulateLoad (loadCaseId, dofIds, loadMatrix);
                    }
                    break;
                }
                default :
                    break;
            }
        }
    }

    // 计算重力荷载
    GravityLoadCalc(dbServiceFactory, loadCaseIds);

    std::cout << "== compute Load Vector finish \n";
}